Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(autonomous_emergency_braking): calculate the object's velocity in the search area #8591

Conversation

ismetatabay
Copy link
Member

@ismetatabay ismetatabay commented Aug 22, 2024

Description

This PR enables the calculation of object velocity within the speed calculation area, using the speed_calculation_expansion_margin parameter.

Related links

Parent Issue:

How was this PR tested?

Psim

Before This PR:

before_pr_new.mp4

After This PR:

after_pr_new.mp4

Stopping for an obstacle. (obstacle_cruise_planner (motion_stop_planner) is disabled)

after_pr2_new.mp4

Notes for reviewers

None.

Interface changes

None.

ROS Parameter Changes

Additions and removals

Change type Parameter Name Type Default Value Description
Added speed_calculation_expansion_margin double 0.5 Lateral margin used to initiate speed calculation for an object.

Effects on system behavior

None.

@github-actions github-actions bot added the component:control Vehicle control algorithms and mechanisms. (auto-assigned) label Aug 22, 2024
Copy link

github-actions bot commented Aug 22, 2024

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

@ismetatabay ismetatabay self-assigned this Aug 22, 2024
@ismetatabay
Copy link
Member Author

Hi @danielsanchezaran -san,
If you have time, could you look at this this PR and Issue?

@ismetatabay ismetatabay added the tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label Aug 22, 2024
Copy link

codecov bot commented Aug 22, 2024

Codecov Report

Attention: Patch coverage is 29.31034% with 41 lines in your changes missing coverage. Please review.

Project coverage is 28.44%. Comparing base (656a78e) to head (b49a8ae).
Report is 5 commits behind head on main.

Files with missing lines Patch % Lines
...autoware_autonomous_emergency_braking/src/node.cpp 29.31% 41 Missing ⚠️
Additional details and impacted files
@@            Coverage Diff             @@
##             main    #8591      +/-   ##
==========================================
- Coverage   28.44%   28.44%   -0.01%     
==========================================
  Files        1314     1315       +1     
  Lines       98290    98340      +50     
  Branches    39986    40011      +25     
==========================================
+ Hits        27958    27971      +13     
- Misses      70296    70332      +36     
- Partials       36       37       +1     
Flag Coverage Δ *Carryforward flag
differential 7.53% <29.31%> (?)
total 28.44% <ø> (+<0.01%) ⬆️ Carriedforward from 656a78e

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

@kyoichi-sugahara
Copy link
Contributor

@ismetatabay
Thank you so much for the PR!
The expanded_ego_polys are assigned using the extended footprint only when use_pointcloud_data_ is true, but when use_pointcloud_data_ is false, they remain empty.
Even when empty, they are still used for inside/outside determination in the within function. Is this intended?

@@ -392,7 +394,9 @@ class AEB : public rclcpp::Node
* @param closest_object Data of the closest object
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ismetatabay
Could you modify doxygen explanation?

@ismetatabay ismetatabay marked this pull request as draft August 23, 2024 13:00
Comment on lines 545 to 554
bool is_point_inside = false;
for (const auto & ego_poly : ego_polys) {
if (bg::within(obj_point, ego_poly)) {
is_point_inside = true;
break;
}
}
if (!is_point_inside) {
return false;
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
bool is_point_inside = false;
for (const auto & ego_poly : ego_polys) {
if (bg::within(obj_point, ego_poly)) {
is_point_inside = true;
break;
}
}
if (!is_point_inside) {
return false;
}
if (!std::any_of(ego_polys.begin(), ego_polys.end(),
[&obj_point](const auto & ego_poly) { return bg::within(obj_point, ego_poly); })) {
return false;
}

It might be a matter of preference, but wouldn't this approach be more simple for what you're trying to accomplish? There's no problem with the current implementation, so it's fine if you don't change it.

@kyoichi-sugahara
Copy link
Contributor

@ismetatabay
Shold I stop reviewing? Asking because you reverted to draft.

@ismetatabay
Copy link
Member Author

Hi @kyoichi-sugahara-san, thank you so much for your comments. If you could continue reviewing, I would be grateful. I converted it to a draft as you mentioned a problem with the usage of expanded_ego_polys. I will check this situation but as I mentioned you can continue reviewing, and then I will update it according to your reviews. Thank you so much in advance 🤝

@kyoichi-sugahara
Copy link
Contributor

@ismetatabay
Ok thank you for quick replay I will leave some comments,

I can confirm from the description that the results have improved, but do you understand why setting the collision result to false when the object's vertices are outside the extended predicted path helps avoid unnecessary activations?
Also, does this change mean that the speed is being calculated appropriately?

Furthermore, if we were to incorporate this change, I think it would be more natural to implement the process of excluding objects that have vertices not contained within the predicted path in the createObjectDataUsingPointCloudClusters function, rather than checking if it's within the predicted path during collision detection.

The reason is that it's preferable to separate the filtering process for potential collision objects from the actual collision detection.
The current PR seems to be performing the filtering process within the function that executes collision detection, which feels inconsistent with the function name and its processing.
By changing, We can avoid the problem in the comment

I apologize if I've overlooked any circumstances and my comments are unreasonable 🙇

@ismetatabay
Copy link
Member Author

ismetatabay commented Aug 23, 2024

@kyoichi-sugahara -san, thank you for your comments. I plan to refactor my implementation and keep this PR as a draft. My current implementation steps are as follows:

-> Use rough point cloud filtering to keep the velocity information of the closest object in this area.
-> If the closest target object is not located in the predicted path, don't calculate a collision.

As you mentioned, the hasCollision function might not need to filter obstacles based on whether they are in the predicted trajectory. I will refactor it accordingly. Thank you again.

@ismetatabay ismetatabay force-pushed the feat/add-tracking-to-search-area branch from 95837c9 to d9d3164 Compare August 23, 2024 14:38
@github-actions github-actions bot added the component:launch Launch files, scripts and initialization tools. (auto-assigned) label Aug 23, 2024
@ismetatabay
Copy link
Member Author

@kyoichi-sugahara -san, my current idea is:

  • Add an is_target attribute to the ObjectData struct (default to true for predicted objects).
  • Add all objects located in the expanded footprint area (with path_footprint_extra_margin) to the object list (to calculate velocity).
  • Check if the object is located in the usual predicted path, then update the object's is_target attribute.
  • If the tracked object is not a target (is in the expanded area for velocity calculation but does not enter the predicted path), then don't call the hasCollision function.

What do you think?

Additionally, I found a mistake in the control.launch.xml launch file and I fixed this topic as well.:

<remap from="~/input/objects" to="/perception/obstacle_segmentation/objects"/>

@kyoichi-sugahara
Copy link
Contributor

@ismetatabay

If the tracked object is not a target (is in the expanded area for velocity calculation but does not enter the predicted path), then don't call the hasCollision function.

So, does this mean that AEB will no longer explicitly react to objects existing outside the normal predicted path?

From referring to the issue (this is super useful result ❤️), I understand that the problem is that the estimation results are unstable for the first few steps after speed calculation begins, causing unnecessary activation.
While reducing the detection targets is a solution, I'm concerned that it might make necessary activations impossible.

From my perspective, I feel the following approaches might be valid solutions:

  • Introduce a mechanism that doesn't utilize the first few steps when the speed estimation is clearly unstable.
  • Prepare separate areas for speed estimation and for detection targets.

What do you think?

@ismetatabay
Copy link
Member Author

@kyoichi-sugahara -san, thank you for your comment ❤️ .

 So, does this mean that AEB will no longer explicitly react to objects existing outside the normal predicted path? 

AEB maintains the same mechanism for reacting to obstacles in the predicted path. The default current implementation uses ego_polys (predicted path + expand_width) for collision detection and expanded_ego_polys (predicted path + expand_width + path_footprint_extra_margin) for point cloud cropping.

AEB drawio

My idea is to calculate the object's velocity when it enters the expanded_ego_polys area. If the object does not enter the ego_polys area (currently used for object detection in the default implementation), we can set the is_target value of this object to false. However, this approach may lead to issues, such as when there are two objects in front of the ego vehicle. If the closer object is within the expanded_ego_polys and the farther one is within the ego_polys, we might not consider the target object (in ego_polys) because it isn't the closest obstacle.


- Introduce a mechanism that doesn't utilize the first few steps when the speed estimation is clearly unstable.
  • I think this might cause a slight delay in reaction time, but it could be a quick implementation.

- Prepare separate areas for speed estimation and for detection targets.
  • I agree with this idea, and I plan to work on it. This area could be expanded_ego_polys, but we need to implement multiple object tracking mechanism I think. I will check. 👍

@kyoichi-sugahara
Copy link
Contributor

@ismetatabay

AEB maintains the same mechanism for reacting to obstacles in the predicted path.

Yes you are right, and sorry I noticed my comment was based on misunderstanding

we might not consider the target object (in ego_polys) because it isn't the closest obstacle.

I understand your concern but, may be the further object can be ignored in my opinion. Only closest object in the predicted path polygon should be considered.

I think this might cause a slight delay in reaction time, but it could be a quick implementation.

yes, that's the concern for this idea which I was discussing with @danielsanchezaran today

I agree with this idea, and I plan to work on it. This area could be expanded_ego_polys, but we need to implement multiple object tracking mechanism I think. I will check. 👍

Thanks! I hope it improves the behavior but still
I feel that it's likely to occur that when a vehicle enters an expanded_ego_polys diagonally, the position of the nearest point doesn't change vertically over time, resulting in a low calculated speed.

@ismetatabay
Copy link
Member Author

@kyoichi-sugahara -san, Thank you so much for your valuable comments and suggestions. My main concern regarding the closest object is that if we calculate and only consider the closest object within separate areas, we might miss the actual target object. I will work on addressing this in the following case:

AEB2 drawio

Since NPC1 is the closest object, we will calculate its speed, but it is not our target object. NPC2 is the target object because it is located on our trajectory. I will check on it.

@ismetatabay ismetatabay force-pushed the feat/add-tracking-to-search-area branch from d2081c2 to f463428 Compare August 27, 2024 12:04
@ismetatabay ismetatabay force-pushed the feat/add-tracking-to-search-area branch from 7f428ab to 7b85937 Compare September 17, 2024 11:03
}
}

// If no target object is found, find the closest object overall
Copy link
Contributor

@danielsanchezaran danielsanchezaran Sep 19, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
// If no target object is found, find the closest object overall
// If no target object is found, find the closest object overall

Sorry for the delay in the reviewing @ismetatabay I am finishing some thing regarding AEB ci/cd and it has taken me a little longer than expected to check for degradation, it should be done by tomorrow, on the mean time could you check this? I think the
const auto closest_object_point_itr calculation part can be deleted, because closest_target_object_itr already returns the closest object even if there are no targets. So i suggest renaming closest_target_object_itr to closest_object_itr and deleting the rest of the redundant code.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No worries, let me check it 👍

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Removed 👍

@danielsanchezaran
Copy link
Contributor

@ismetatabay it looks like there might be some trouble with using arc coordinates. I cannot really tell why, but detection seems to happen when it should not. I am using the default params.

cap-.2024-09-19-18-54-28.online-video-cutter.com.mp4

@ismetatabay
Copy link
Member Author

Hello @danielsanchezaran -san,

I couldn't reproduce the issue you encountered with the planning simulator. If possible, could you share the bag and map files? so I can check the problem

2024-09-19.13-44-04.mp4

@ismetatabay
Copy link
Member Author

@danielsanchezaran
I have concerns about the ground segmentation performance in your video. Some false positives from ground segmentation in the target area might trigger the emergency. Could you check it?

image

image

@ismetatabay ismetatabay force-pushed the feat/add-tracking-to-search-area branch from 7b85937 to 329b4f7 Compare September 19, 2024 13:39
@danielsanchezaran
Copy link
Contributor

danielsanchezaran commented Sep 20, 2024

@danielsanchezaran I have concerns about the ground segmentation performance in your video. Some false positives from ground segmentation in the target area might trigger the emergency. Could you check it?

image

image

@ismetatabay thank you for your input. I will try and see if I can reproduce the problem reliably or if it is a tuning problem. The ground segmentation does indeed look suspicious, but I also tried setting the minimum cluster height to 0.1 m to mitigate that, with no luck. Would it be possible to you to do some tests using AWsim? or maybe with a rosbag you have with similar conditions (vehicles close to the ego on both sides)

@danielsanchezaran
Copy link
Contributor

danielsanchezaran commented Sep 20, 2024

@ismetatabay I discovered what was the issue.

Some of the MPC path points are virtually the same when the ego is starting to move, that causes the calcLatoffset function to misbehave and not caluclate the proper lateral distance. To solve the issue, we can omit mpc path points that are too close to each other:

--- a/control/autoware_autonomous_emergency_braking/src/node.cpp
+++ b/control/autoware_autonomous_emergency_braking/src/node.cpp
@@ -709,6 +709,9 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj)
for (size_t i = 0; i < predicted_traj.points.size(); ++i) {
geometry_msgs::msg::Pose map_pose;
tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped.value());

  • if (autoware::universe_utils::calcDistance2d(path.back(), map_pose) < 1e-2) {
  •  continue;
    
  • }
    path.push_back(map_pose);

I would like you to please add this change and also make the minimum distance between points 1e-2 for the IMU path, for consistency.

Also, please note that calcLateralOffset might return NaN, so please handle that case by adding a
if (std::isnan(lateral_offset)) continue;
right after calculating the lat offset on the AEB::getClosestObjectsOnPath function.

As a note, the calcLateralOffset function checks if 2 points are too close, but the epsilon value used (1e-8 might be too small. I hopefully will make a PR next week to fix it).

On the mean time, I will approve this PR, I think it is a great addition to AEB and I am grateful for it, but please fix the issues I pointed out before merging!!!!

Copy link
Contributor

@danielsanchezaran danielsanchezaran left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Approved, but please fix the issues on my last comment before merging, the rest LGTM

@ismetatabay ismetatabay force-pushed the feat/add-tracking-to-search-area branch from 329b4f7 to eae0170 Compare September 20, 2024 11:57
@ismetatabay
Copy link
Member Author

@danielsanchezaran Thank you so much for your efforts. Before merging it, I want to test it on a real vehicle as well. I will update you. Have a good weekend!

@danielsanchezaran
Copy link
Contributor

@danielsanchezaran Thank you so much for your efforts. Before merging it, I want to test it on a real vehicle as well. I will update you. Have a good weekend!

Thank you @ismetatabay any idea when you will you be able to do the tests?

@ismetatabay ismetatabay force-pushed the feat/add-tracking-to-search-area branch from eae0170 to f85fa01 Compare September 24, 2024 17:39
path.push_back(map_pose);

if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) {
break;
}
}
time_keeper_->end_track("createPath");
if (path.empty()) return std::nullopt;
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@danielsanchezaran Hello, I tested it today and things works as expected in lane_driving scenario. However in parking scenario, sometimes the trajectory will be very short and the returned path will be empty. To fix it I pushed this commit: f85fa01

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ismetatabay thank you, that is great. So I understand you tested in on the real vehicle right?

PD: I added a small recommendation regarding this line.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@danielsanchezaran Yes, I tested it with speed_calculation_expansion_margin set to 4.0, and we didn’t stop for any overtaken NPCs during normal lane driving in a campus environment, even though we stopped for a static obstacle at 30 km/h (with -5.0 m/s² emergency deceleration and obstacle_cruise_planner disabled during the obstacle stop test). Also I pushed your suggestion 👍 65f869b . If it is okay for you, I think we can merge it.

Copy link
Contributor

@danielsanchezaran danielsanchezaran Sep 26, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ismetatabay thank you, it looks great, any chance you might update the discussion about AEB with the new tests ?https://github.com/orgs/autowarefoundation/discussions/5020#discussioncomment-10312780

I have merged the PR, thank you for such a good contribution!

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@danielsanchezaran Thank you as well. Our research vehicle is undergoing a hardware upgrade, and I will likely update the report next week. 👍

Copy link
Contributor

@danielsanchezaran danielsanchezaran left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe this is more concise

path.push_back(map_pose);

if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) {
break;
}
}
time_keeper_->end_track("createPath");
if (path.empty()) return std::nullopt;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
if (path.empty()) return std::nullopt;
return (!path.empty()) ? std::make_optional(path) : std::nullopt;

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done 👍

@ismetatabay ismetatabay force-pushed the feat/add-tracking-to-search-area branch from f85fa01 to 65f869b Compare September 25, 2024 07:14
Signed-off-by: ismetatabay <[email protected]>
Signed-off-by: ismetatabay <[email protected]>
Signed-off-by: ismetatabay <[email protected]>
Signed-off-by: ismetatabay <[email protected]>
Signed-off-by: ismetatabay <[email protected]>
@ismetatabay ismetatabay force-pushed the feat/add-tracking-to-search-area branch from 65f869b to b49a8ae Compare September 25, 2024 09:33
@danielsanchezaran danielsanchezaran merged commit 8f0e511 into autowarefoundation:main Sep 26, 2024
31 of 32 checks passed
danielsanchezaran pushed a commit to tier4/autoware.universe that referenced this pull request Sep 26, 2024
…n the search area (autowarefoundation#8591)

* refactor PR

Signed-off-by: ismetatabay <[email protected]>

* WIP

Signed-off-by: ismetatabay <[email protected]>

* change using polygon to lateral offset

Signed-off-by: ismetatabay <[email protected]>

* improve code

Signed-off-by: ismetatabay <[email protected]>

* remove redundant code

Signed-off-by: ismetatabay <[email protected]>

* skip close points in MPC path generation

Signed-off-by: ismetatabay <[email protected]>

* fix empty path points in short parking scenario

Signed-off-by: ismetatabay <[email protected]>

* fix readme conflicts

Signed-off-by: ismetatabay <[email protected]>

---------

Signed-off-by: ismetatabay <[email protected]>
@ismetatabay ismetatabay deleted the feat/add-tracking-to-search-area branch September 26, 2024 06:39
prakash-kannaiah pushed a commit to prakash-kannaiah/autoware.universe that referenced this pull request Oct 9, 2024
…n the search area (autowarefoundation#8591)

* refactor PR

Signed-off-by: ismetatabay <[email protected]>

* WIP

Signed-off-by: ismetatabay <[email protected]>

* change using polygon to lateral offset

Signed-off-by: ismetatabay <[email protected]>

* improve code

Signed-off-by: ismetatabay <[email protected]>

* remove redundant code

Signed-off-by: ismetatabay <[email protected]>

* skip close points in MPC path generation

Signed-off-by: ismetatabay <[email protected]>

* fix empty path points in short parking scenario

Signed-off-by: ismetatabay <[email protected]>

* fix readme conflicts

Signed-off-by: ismetatabay <[email protected]>

---------

Signed-off-by: ismetatabay <[email protected]>
Signed-off-by: prakash-kannaiah <[email protected]>
danielsanchezaran pushed a commit to tier4/autoware.universe that referenced this pull request Oct 28, 2024
…n the search area (autowarefoundation#8591)

* refactor PR

Signed-off-by: ismetatabay <[email protected]>

* WIP

Signed-off-by: ismetatabay <[email protected]>

* change using polygon to lateral offset

Signed-off-by: ismetatabay <[email protected]>

* improve code

Signed-off-by: ismetatabay <[email protected]>

* remove redundant code

Signed-off-by: ismetatabay <[email protected]>

* skip close points in MPC path generation

Signed-off-by: ismetatabay <[email protected]>

* fix empty path points in short parking scenario

Signed-off-by: ismetatabay <[email protected]>

* fix readme conflicts

Signed-off-by: ismetatabay <[email protected]>

---------

Signed-off-by: ismetatabay <[email protected]>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
component:control Vehicle control algorithms and mechanisms. (auto-assigned) tag:run-build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) type:documentation Creating or refining documentation. (auto-assigned)
Projects
No open projects
Status: Done
Development

Successfully merging this pull request may close these issues.

AEB module incorrectly calculates target obstacle's velocity at first calculation
3 participants